SE2.h

Go to the documentation of this file.

// WOLF - Copyright (C) 2020-2026
// Institut de Robòtica i Informàtica Industrial, CSIC-UPC.
// Authors: Joan Solà Ortega (jsola@iri.upc.edu) and
// Joan Vallvé Navarro (jvallve@iri.upc.edu)
// All rights reserved.
//
// This file is part of WOLF: http://www.iri.upc.edu/wolf
// WOLF is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License version 3
// as published by the Free Software Foundation.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#pragma once
 
#include "core/common/wolf.h"
#include "core/math/rotations.h"
#include "core/composite/vector_composite.h"
 
#include <Eigen/Geometry>
#include <Eigen/Dense>
 
/*
 * The functions in this file are related to manipulations of Delta motion magnitudes used in 2d motion.
 *
 * The Delta is defined as a simple 2d pose with the rotation expressed as an angle,
 *     Delta = [Dp, Do]
 * with
 *     Dp : position delta
 *     Do : angle delta (always in (-pi, pi])
 * and its tangent vector (Lie algebra) is tau = [rho, theta].
 *
 * The functions are listed below:
 *
 *   - identity:    I  = Delta at the origin, with Dp = [0,0]; Do = 0, so that D (+) I = I (+) D = D
 *   - inverse:     so that D (+) D.inv = D.inv (+) D = I
 *   - compose:     Dc = D1 (+) D2
 *   - between:     Db = D2 (-) D1 := D1.inv (+) D2, so that D2 = D1 (+) Db
 *   - exp:         go from tangent space to delta manifold (SE(2) exponential map)
 *   - log:         go from delta manifold to tangent space (SE(2) logarithmic map)
 *   - plus:        block-wise: D2 = [Dp1 + dp, Do1 + do]
 *   - minus:       block-wise: d  = [Dp2 - Dp1, Do2 - Do1], so that D2 = plus(D1, d)
 *   - interpolate: D  = D1 (+) exp( t * log( D1.inv (+) D2 ) )
 *   - toSE2:       3x3 homogeneous matrix
 *   - jac_SE2_left, jac_SE2_right, jac_SE2_left_inv, jac_SE2_right_inv: Jacobians of SE(2)
 *
 * Each function is provided (when applicable) with the following APIs:
 *   - by parts, with the rotation as a scalar angle
 *   - by parts, with the rotation as a vector (1-vector with the angle)
 *   - full 3-vector [p, o] as output parameter and as return value
 *   - full 3-vector with Jacobians. Jacobians are block-wise: w.r.t. perturbations [dp, do] with additive dp
 *     and do, i.e. consistent with plus() and minus().
 *   - VectorComposite with keys 'P' and 'O', as output parameter and as return value
 *
 * ------------------------------------------------------------------------------------------
 *
 * Some of the functions below are based on:
 *
 * [1] J. Sola et. al. "A micro Lie theory for state estimation in robotics", tech rep. IRI 2018,
 * https://arxiv.org/pdf/1812.01537.pdf
 */
 
namespace wolf
{
namespace SO2
{
using namespace Eigen;
 
template <typename T>
inline Matrix<T, 2, 2> exp(const T theta)
{
    return Rotation2D<T>(theta).matrix();
}
 
}  // namespace SO2
 
namespace SE2
{
using namespace Eigen;
 
template <class T>
inline Matrix<T, 2, 2> skew(const T& t)
{
    return (Matrix<T, 2, 2>() << (T)0, -t, t, (T)0).finished();
}
 
template <class D>
inline Matrix<typename D::Scalar, 2, 1> skewed(const MatrixBase<D>& t)
{
    return Matrix<typename D::Scalar, 2, 1>(-t(1), t(0));
}
 
template <typename T>
inline Matrix<T, 2, 2> V_helper(const T theta)
{
    using std::abs;
    T s;    //   sin(theta)   / theta
    T c_1;  // (1-cos(theta)) / theta
 
    if (abs(theta) > T(Constants::EPS))
    {
        // [1] eq. 158, with 1 - cos(theta) = 2 sin^2(theta/2) (numerically stable)
        T sin_half = sin(theta / T(2.0));
        s          = sin(theta) / theta;
        c_1        = T(2.0) * sin_half * sin_half / theta;
    }
    else
    {
        // approx of [1] eq. 158
        s   = T(1.0) - theta * theta / T(6.0);  // sin(x) ~= x - x^3/6
        c_1 = theta / T(2.0);                   // cos(x) ~= 1 - x^2/2
    }
 
    return (Matrix<T, 2, 2>() << s, -c_1, c_1, s).finished();
}
 
template <typename T>
inline Matrix<T, 2, 2> V_inv_helper(const T theta)
{
    using std::abs;
    T a;  // theta * sin(theta) / (2 * (1 - cos(theta))) = (theta / 2) / tan(theta / 2)
 
    if (abs(theta) > T(Constants::EPS))
        a = (theta / T(2.0)) / tan(theta / T(2.0));
    else
        a = T(1.0) - theta * theta / T(12.0);
 
    return (Matrix<T, 2, 2>() << a, theta / T(2.0), -theta / T(2.0), a).finished();
}
 
template <class D, typename T>
inline Matrix<T, 2, 1> J_Vp_theta(const MatrixBase<D>& p, const T theta)
{
    MatrixSizeCheck<2, 1>::check(p);
 
    /* Jacobian of t = V(theta)*p wrt theta, with V = [s, -c_1; c_1, s]:
     *   dV/dtheta = [ds, -dc_1; dc_1, ds]
     *   ds   = d(sin(theta)/theta)/dtheta     = (theta cos(theta) - sin(theta)) / theta^2
     *   dc_1 = d((1-cos(theta))/theta)/dtheta = (theta sin(theta) - 1 + cos(theta)) / theta^2
     * (Taylor series for small angles, where the closed forms are numerically unstable)
     */
    using std::abs;
    T ds, dc_1;
    if (abs(theta) > T(1e-2))
    {
        T s_th   = sin(theta);
        T c_th   = cos(theta);
        T theta2 = theta * theta;
        ds       = (theta * c_th - s_th) / theta2;
        dc_1     = (theta * s_th - T(1.0) + c_th) / theta2;
    }
    else
    {
        T theta2 = theta * theta;
        ds       = theta * (-T(1. / 3.) + theta2 * (T(1. / 30.) - theta2 / T(840.)));
        dc_1     = T(0.5) + theta2 * (-T(1. / 8.) + theta2 * (T(1. / 144.) - theta2 / T(5760.)));
    }
 
    return Matrix<T, 2, 1>(ds * p(0) - dc_1 * p(1), dc_1 * p(0) + ds * p(1));
}
 
// JACOBIANS OF SE(2)
 
template <typename D>
inline Matrix<typename D::Scalar, 3, 3> jac_SE2_left(const MatrixBase<D>& tangent)
{
    MatrixSizeCheck<3, 1>::check(tangent);
    typedef typename D::Scalar T;
 
    T x     = tangent(0);
    T y     = tangent(1);
    T theta = tangent(2);
 
    /* third column: [x B + y A, -x A + y B] with
     *   A = (1 - cos(theta)) / theta^2
     *   B = (theta - sin(theta)) / theta^2
     * (Taylor series for small angles, where the closed forms are numerically unstable)
     */
    using std::abs;
    T A, B;
    if (abs(theta) > T(1e-2))
    {
        T sin_half = sin(theta / T(2.0));
        T theta2   = theta * theta;
        A          = T(2.0) * sin_half * sin_half / theta2;
        B          = (theta - sin(theta)) / theta2;
    }
    else
    {
        T theta2 = theta * theta;
        A        = T(0.5) + theta2 * (-T(1. / 24.) + theta2 / T(720.));
        B        = theta * (T(1. / 6.) + theta2 * (-T(1. / 120.) + theta2 / T(5040.)));
    }
 
    Matrix<T, 3, 3> Jl;
    Jl.template topLeftCorner<2, 2>() = V_helper(theta);
    Jl(0, 2)                          = x * B + y * A;
    Jl(1, 2)                          = -x * A + y * B;
    Jl(2, 0)                          = T(0);
    Jl(2, 1)                          = T(0);
    Jl(2, 2)                          = T(1);
    return Jl;
}
 
template <typename D>
inline Matrix<typename D::Scalar, 3, 3> jac_SE2_right(const MatrixBase<D>& tangent)
{
    return jac_SE2_left(Matrix<typename D::Scalar, 3, 1>(-tangent));
}
 
template <typename D>
inline Matrix<typename D::Scalar, 3, 3> jac_SE2_left_inv(const MatrixBase<D>& tangent)
{
    typedef typename D::Scalar T;
 
    // Jl = [A, b; 0, 1] --> Jl.inv = [A.inv, -A.inv * b; 0, 1]
    Matrix<T, 3, 3> Jl                     = jac_SE2_left(tangent);
    Matrix<T, 2, 2> A_inv                  = V_inv_helper(T(tangent(2)));
    Matrix<T, 3, 3> Jl_inv                 = Matrix<T, 3, 3>::Identity();
    Jl_inv.template topLeftCorner<2, 2>()  = A_inv;
    Jl_inv.template topRightCorner<2, 1>() = -A_inv * Jl.template topRightCorner<2, 1>();
    return Jl_inv;
}
 
template <typename D>
inline Matrix<typename D::Scalar, 3, 3> jac_SE2_right_inv(const MatrixBase<D>& tangent)
{
    return jac_SE2_left_inv(Matrix<typename D::Scalar, 3, 1>(-tangent));
}
 
// IDENTITY
 
template <typename D>
inline void identity(MatrixBase<D>& p, typename D::Scalar& o)
{
    MatrixSizeCheck<2, 1>::check(p);
    p.setZero();
    o = typename D::Scalar(0);
}
 
template <typename D1, typename D2>
inline void identity(MatrixBase<D1>& p, MatrixBase<D2>& o)
{
    MatrixSizeCheck<1, 1>::check(o);
    identity(p, o(0));
}
 
template <typename D>
inline void identity(MatrixBase<D>& d)
{
    MatrixSizeCheck<3, 1>::check(d);
 
    Map<Matrix<typename D::Scalar, 2, 1>> p(&d(0));
    identity(p, d(2));
}
 
template <typename T = double>
inline Matrix<T, 3, 1> identity()
{
    Matrix<T, 3, 1> ret;
    identity(ret);
    return ret;
}
 
inline void identity(VectorComposite& v)
{
    v.clear();
    v['P'].resize(2);
    v['O'].resize(1);
    identity(v['P'], v['O']);
}
 
inline VectorComposite identityComposite()
{
    VectorComposite v;
    identity(v);
    return v;
}
 
// INVERSE
 
template <typename D1, typename D2>
inline void inverse(const MatrixBase<D1>& p, const typename D1::Scalar& o, MatrixBase<D2>& ip, typename D2::Scalar& io)
{
    MatrixSizeCheck<2, 1>::check(p);
    MatrixSizeCheck<2, 1>::check(ip);
 
    ip = SO2::exp(-o) * -p;
    io = pi2pi(-o);
}
 
template <typename D1, typename D2, typename D3, typename D4>
inline void inverse(const MatrixBase<D1>& p, const MatrixBase<D2>& o, MatrixBase<D3>& ip, MatrixBase<D4>& io)
{
    MatrixSizeCheck<1, 1>::check(o);
    MatrixSizeCheck<1, 1>::check(io);
 
    inverse(p, o(0), ip, io(0));
}
 
template <typename D1, typename D2>
inline void inverse(const MatrixBase<D1>& d, MatrixBase<D2>& id)
{
    MatrixSizeCheck<3, 1>::check(d);
    MatrixSizeCheck<3, 1>::check(id);
 
    const auto                             p = d.template head<2>();
    Map<Matrix<typename D2::Scalar, 2, 1>> ip(&id(0));
 
    inverse(p, d(2), ip, id(2));
}
 
template <typename D>
inline Matrix<typename D::Scalar, 3, 1> inverse(const MatrixBase<D>& d)
{
    Matrix<typename D::Scalar, 3, 1> id;
    inverse(d, id);
    return id;
}
 
inline void inverse(const VectorComposite& v, VectorComposite& c)
{
    c['P'].resize(2);
    c['O'].resize(1);
    inverse(v.at('P'), v.at('O'), c['P'], c['O']);
}
 
inline VectorComposite inverse(const VectorComposite& v)
{
    VectorComposite c;
    inverse(v, c);
    return c;
}
 
// COMPOSE
 
template <typename D1, typename D2, typename D3>
inline void compose(const MatrixBase<D1>&      p1,
                    const typename D1::Scalar& o1,
                    const MatrixBase<D2>&      p2,
                    const typename D2::Scalar& o2,
                    MatrixBase<D3>&            pc,
                    typename D3::Scalar&       oc)
{
    MatrixSizeCheck<2, 1>::check(p1);
    MatrixSizeCheck<2, 1>::check(p2);
    MatrixSizeCheck<2, 1>::check(pc);
 
    // tc = t1 + R1 t2
    pc = p1 + SO2::exp(o1) * p2;
 
    // Rc = R1 R2 --> theta = theta1 + theta2
    oc = pi2pi(o1 + o2);
}
 
template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void compose(const MatrixBase<D1>& p1,
                    const MatrixBase<D2>& o1,
                    const MatrixBase<D4>& p2,
                    const MatrixBase<D5>& o2,
                    MatrixBase<D7>&       pc,
                    MatrixBase<D8>&       oc)
{
    MatrixSizeCheck<1, 1>::check(o1);
    MatrixSizeCheck<1, 1>::check(o2);
    MatrixSizeCheck<1, 1>::check(oc);
 
    compose(p1, o1(0), p2, o2(0), pc, oc(0));
}
 
template <class D1, class D2, class D3>
inline void compose(const MatrixBase<D1>& _delta1,
                    const MatrixBase<D2>& _delta2,
                    MatrixBase<D3>&       _delta1_compose_delta2)
{
    MatrixSizeCheck<3, 1>::check(_delta1);
    MatrixSizeCheck<3, 1>::check(_delta2);
    MatrixSizeCheck<3, 1>::check(_delta1_compose_delta2);
 
    const auto                             p1 = _delta1.template head<2>();
    const auto                             p2 = _delta2.template head<2>();
    Map<Matrix<typename D3::Scalar, 2, 1>> pc(&_delta1_compose_delta2(0));
 
    compose(p1, _delta1(2), p2, _delta2(2), pc, _delta1_compose_delta2(2));
}
 
template <class D1, class D2>
inline Matrix<typename D1::Scalar, 3, 1> compose(const MatrixBase<D1>& _delta1, const MatrixBase<D2>& _delta2)
{
    Matrix<typename D1::Scalar, 3, 1> delta1_compose_delta2;
    compose(_delta1, _delta2, delta1_compose_delta2);
    return delta1_compose_delta2;
}
 
template <class D1, class D2, class D3, class D4, class D5>
inline void compose(const MatrixBase<D1>& _delta1,
                    const MatrixBase<D2>& _delta2,
                    MatrixBase<D3>&       _delta1_compose_delta2,
                    MatrixBase<D4>&       _J_compose_delta1,
                    MatrixBase<D5>&       _J_compose_delta2)
{
    MatrixSizeCheck<3, 1>::check(_delta1);
    MatrixSizeCheck<3, 1>::check(_delta2);
    MatrixSizeCheck<3, 1>::check(_delta1_compose_delta2);
    MatrixSizeCheck<3, 3>::check(_J_compose_delta1);
    MatrixSizeCheck<3, 3>::check(_J_compose_delta2);
 
    typedef typename D1::Scalar T;
 
    Matrix<T, 2, 2> R1 = SO2::exp(T(_delta1(2)));  // need temporary
 
    /* Jacobians in composite form [1] see eqs. (89, 125, 129, 130)
     *
     * resulting delta is:
     *
     * tc = t1 + R1 t2  (*)
     * Rc = R1 R2       (**)
     *
     * Jacobians have the form:
     *
     * [ J_t_t   J_t_R ]
     * [ J_r_t   J_R_R ]
     *
     * Jacobian blocks are:
     *
     * J_tc_t1 = I                                  trivial from (*)
     * J_tc_R1 = R1 [1]x t2 = R1 (-t2.y ; t2.x)     see [1]: eq. (129)
     *
     * J_Rc_t1 = (0 0)                              trivial from (**)
     * J_Rc_R1 = 1                                  see [1]: eq. (125)
     *
     * J_tc_t2 = R1                                 see [1]: eq. (130)
     * J_tc_R2 = 0                                  trivial from (*)
     *
     * J_Rc_t2 = 0                                  trivial from (**)
     * J_Rc_R2 = 1                                  see [1]: eq. (125)
     */
 
    _J_compose_delta1.setIdentity();
    _J_compose_delta1.template block<2, 1>(0, 2) = R1 * skewed(_delta2.template head<2>());
 
    _J_compose_delta2.setIdentity(3, 3);
    _J_compose_delta2.template block<2, 2>(0, 0) = R1;
 
    // compose deltas -- done here to avoid aliasing when calling with input `_delta1` and result referencing the same
    // variable
    compose(_delta1, _delta2, _delta1_compose_delta2);
}
 
inline void compose(const VectorComposite& _x1, const VectorComposite& _x2, VectorComposite& _c)
{
    _c['P'].resize(2);
    _c['O'].resize(1);
    compose(_x1.at('P'), _x1.at('O'), _x2.at('P'), _x2.at('O'), _c['P'], _c['O']);
}
 
inline VectorComposite compose(const VectorComposite& x1, const VectorComposite& x2)
{
    VectorComposite c;
    compose(x1, x2, c);
    return c;
}
 
// inline void compose(const VectorComposite& _x1,
//                     const VectorComposite& _x2,
//                     VectorComposite&       _c,
//                     MatrixComposite&       _J_c_x1,
//                     MatrixComposite&       _J_c_x2)
// {
//     const auto& p1 = _x1.at('P');
//     const auto& a1 = _x1.at('O')(0);  // angle
//     Matrix2d    R1 = SO2::exp(a1);    // need temporary
 
//     const auto& p2 = _x2.at('P');
//     const auto& a2 = _x2.at('O')(0);  // angle
 
//     /* Jacobians in composite form [1] see eqs. (89, 125, 129, 130)
//      * (see compose() above)
//      */
 
//     _J_c_x1.clear();
//     _J_c_x1.emplace('P', 'P', Matrix2d::Identity());
//     _J_c_x1.emplace('P', 'O', MatrixXd(R1 * skewed(p2)));
//     _J_c_x1.emplace('O', 'P', RowVector2d(0, 0));
//     _J_c_x1.emplace('O', 'O', Matrix1d(1));
 
//     _J_c_x2.clear();
//     _J_c_x2.emplace('P', 'P', R1);
//     _J_c_x2.emplace('P', 'O', Vector2d(0, 0));
//     _J_c_x2.emplace('O', 'P', RowVector2d(0, 0));
//     _J_c_x2.emplace('O', 'O', Matrix1d(1));
 
//     // Actually compose the vectors here. This avoids aliasing in case this is called with 'c' coinciding with 'x1'
//     or
//     // 'x2'.
 
//     // tc = t1 + R1 t2
//     _c['P'] = p1 + R1 * p2;
 
//     // Rc = R1 R2 --> theta = theta1 + theta2
//     _c['O'] = Matrix1d(pi2pi(a1 + a2));
// }
 
// BETWEEN
 
template <typename D1, typename D2, typename D3>
inline void between(const MatrixBase<D1>&      p1,
                    const typename D1::Scalar& o1,
                    const MatrixBase<D2>&      p2,
                    const typename D2::Scalar& o2,
                    MatrixBase<D3>&            p12,
                    typename D3::Scalar&       o12)
{
    MatrixSizeCheck<2, 1>::check(p1);
    MatrixSizeCheck<2, 1>::check(p2);
    MatrixSizeCheck<2, 1>::check(p12);
 
    p12 = SO2::exp(-o1) * (p2 - p1);
    o12 = pi2pi(o2 - o1);
}
 
template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void between(const MatrixBase<D1>& p1,
                    const MatrixBase<D2>& o1,
                    const MatrixBase<D4>& p2,
                    const MatrixBase<D5>& o2,
                    MatrixBase<D7>&       p12,
                    MatrixBase<D8>&       o12)
{
    MatrixSizeCheck<1, 1>::check(o1);
    MatrixSizeCheck<1, 1>::check(o2);
    MatrixSizeCheck<1, 1>::check(o12);
 
    between(p1, o1(0), p2, o2(0), p12, o12(0));
}
 
template <typename D1, typename D2, typename D3>
inline void between(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBase<D3>& d12)
{
    MatrixSizeCheck<3, 1>::check(d1);
    MatrixSizeCheck<3, 1>::check(d2);
    MatrixSizeCheck<3, 1>::check(d12);
 
    const auto                             p1 = d1.template head<2>();
    const auto                             p2 = d2.template head<2>();
    Map<Matrix<typename D3::Scalar, 2, 1>> p12(&d12(0));
 
    between(p1, d1(2), p2, d2(2), p12, d12(2));
}
 
template <typename D1, typename D2>
inline Matrix<typename D1::Scalar, 3, 1> between(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2)
{
    Matrix<typename D1::Scalar, 3, 1> d12;
    between(d1, d2, d12);
    return d12;
}
 
inline void between(const VectorComposite& x1, const VectorComposite& x2, VectorComposite& x12)
{
    x12['P'].resize(2);
    x12['O'].resize(1);
    between(x1.at('P'), x1.at('O'), x2.at('P'), x2.at('O'), x12['P'], x12['O']);
}
 
inline VectorComposite between(const VectorComposite& x1, const VectorComposite& x2)
{
    VectorComposite x12;
    between(x1, x2, x12);
    return x12;
}
 
// EXP
 
template <typename D1, typename D2>
inline void exp(const MatrixBase<D1>& rho, const typename D1::Scalar& theta, MatrixBase<D2>& p, typename D2::Scalar& o)
{
    MatrixSizeCheck<2, 1>::check(rho);
    MatrixSizeCheck<2, 1>::check(p);
 
    // [1] eq. 156
    p = V_helper(theta) * rho;
    o = pi2pi(theta);
}
 
template <typename D1, typename D2, typename D3, typename D4>
inline void exp(const MatrixBase<D1>& rho, const MatrixBase<D2>& theta, MatrixBase<D3>& p, MatrixBase<D4>& o)
{
    MatrixSizeCheck<1, 1>::check(theta);
    MatrixSizeCheck<1, 1>::check(o);
 
    exp(rho, theta(0), p, o(0));
}
 
template <class D1, class D2>
inline void exp(const MatrixBase<D1>& _tau, MatrixBase<D2>& _delta)
{
    MatrixSizeCheck<3, 1>::check(_tau);
    MatrixSizeCheck<3, 1>::check(_delta);
 
    const auto                             rho = _tau.template head<2>();
    Map<Matrix<typename D2::Scalar, 2, 1>> p(&_delta(0));
 
    exp(rho, _tau(2), p, _delta(2));
}
 
template <typename D>
inline Matrix<typename D::Scalar, 3, 1> exp(const MatrixBase<D>& _tau)
{
    Matrix<typename D::Scalar, 3, 1> _delta;
    exp(_tau, _delta);
    return _delta;
}
 
template <class D1, class D2, class D3>
inline void exp(const MatrixBase<D1>& _tau, MatrixBase<D2>& _delta, MatrixBase<D3>& _J_delta_tau)
{
    MatrixSizeCheck<3, 1>::check(_tau);
    MatrixSizeCheck<3, 1>::check(_delta);
    MatrixSizeCheck<3, 3>::check(_J_delta_tau);
 
    typedef typename D1::Scalar T;
 
    Matrix<T, 3, 1> tau = _tau;  // copy (tau and delta may alias)
 
    // Jacobian is the composite definition [1] eq. 89, with jacobian blocks:
    //   J_Vp_p = V: see V_helper, eq. 158
    //   J_Vp_theta: see fcn helper
    //   J_theta_theta = 1; eq. 126
    _J_delta_tau.template topLeftCorner<2, 2>()  = V_helper(tau(2));
    _J_delta_tau.template topRightCorner<2, 1>() = J_Vp_theta(tau.template head<2>(), tau(2));
    _J_delta_tau.template bottomRows<1>() << T(0), T(0), T(1);
 
    exp(tau, _delta);
}
 
inline void exp(const VectorComposite& _tau, VectorComposite& _delta)
{
    _delta['P'].resize(2);
    _delta['O'].resize(1);
    exp(_tau.at('P'), _tau.at('O'), _delta['P'], _delta['O']);
}
 
inline VectorComposite exp(const VectorComposite& tau)
{
    VectorComposite res;
    exp(tau, res);
    return res;
}
 
// inline void exp(const VectorComposite& _tau, VectorComposite& _delta, MatrixComposite& _J_delta_tau)
// {
//     // [1] eq. 156
//     const auto& p     = _tau.at('P');
//     const auto& theta = pi2pi(_tau.at('O')(0));
//     Matrix2d    V     = V_helper(theta);
 
//     _delta['P'] = V * p;
//     _delta['O'] = Matrix1d(theta);
 
//     // Jacobian follows the composite definition in [1] eq. 89, with jacobian blocks:
//     //   J_Vp_p = V: see V_helper, eq. 158
//     //   J_Vp_theta: see fcn helper
//     //   J_theta_theta = 1; eq. 126
//     _J_delta_tau.clear();
//     _J_delta_tau.emplace('P', 'P', V);
//     _J_delta_tau.emplace('P', 'O', J_Vp_theta(p, theta));
//     _J_delta_tau.emplace('O', 'P', RowVector2d(0.0, 0.0));
//     _J_delta_tau.emplace('O', 'O', Matrix1d(1));
// }
 
// LOG
 
template <typename D1, typename D2>
inline void log(const MatrixBase<D1>& p, const typename D1::Scalar& o, MatrixBase<D2>& rho, typename D2::Scalar& theta)
{
    MatrixSizeCheck<2, 1>::check(p);
    MatrixSizeCheck<2, 1>::check(rho);
 
    typename D1::Scalar th = pi2pi(o);
    rho                    = V_inv_helper(th) * p;
    theta                  = th;
}
 
template <typename D1, typename D2, typename D3, typename D4>
inline void log(const MatrixBase<D1>& p, const MatrixBase<D2>& o, MatrixBase<D3>& rho, MatrixBase<D4>& theta)
{
    MatrixSizeCheck<1, 1>::check(o);
    MatrixSizeCheck<1, 1>::check(theta);
 
    log(p, o(0), rho, theta(0));
}
 
template <class D1, class D2>
inline void log(const MatrixBase<D1>& _delta, MatrixBase<D2>& _tau)
{
    MatrixSizeCheck<3, 1>::check(_delta);
    MatrixSizeCheck<3, 1>::check(_tau);
 
    const auto                             p = _delta.template head<2>();
    Map<Matrix<typename D2::Scalar, 2, 1>> rho(&_tau(0));
 
    log(p, _delta(2), rho, _tau(2));
}
 
template <typename D>
inline Matrix<typename D::Scalar, 3, 1> log(const MatrixBase<D>& _delta)
{
    Matrix<typename D::Scalar, 3, 1> _tau;
    log(_delta, _tau);
    return _tau;
}
 
template <class D1, class D2, class D3>
inline void log(const MatrixBase<D1>& _delta, MatrixBase<D2>& _tau, MatrixBase<D3>& _J_tau_delta)
{
    MatrixSizeCheck<3, 1>::check(_delta);
    MatrixSizeCheck<3, 1>::check(_tau);
    MatrixSizeCheck<3, 3>::check(_J_tau_delta);
 
    typedef typename D1::Scalar T;
 
    Matrix<T, 3, 1> tau;
    log(_delta, tau);
 
    // inverse of the jacobian of exp(): J_exp = [V, J_Vp_theta; 0, 1] --> J_log = [V.inv, -V.inv * J_Vp_theta; 0, 1]
    Matrix<T, 2, 2> V_inv = V_inv_helper(tau(2));
 
    _J_tau_delta.template topLeftCorner<2, 2>()  = V_inv;
    _J_tau_delta.template topRightCorner<2, 1>() = -V_inv * J_Vp_theta(tau.template head<2>(), tau(2));
    _J_tau_delta.template bottomRows<1>() << T(0), T(0), T(1);
 
    _tau = tau;
}
 
inline void log(const VectorComposite& _delta, VectorComposite& _tau)
{
    _tau['P'].resize(2);
    _tau['O'].resize(1);
    log(_delta.at('P'), _delta.at('O'), _tau['P'], _tau['O']);
}
 
inline VectorComposite log(const VectorComposite& delta)
{
    VectorComposite tau;
    log(delta, tau);
    return tau;
}
 
// PLUS (block-wise)
 
template <typename D1, typename D2, typename D3>
inline void plus(const MatrixBase<D1>&      p1,
                 const typename D1::Scalar& o1,
                 const MatrixBase<D2>&      p2,
                 const typename D2::Scalar& o2,
                 MatrixBase<D3>&            plus_p,
                 typename D3::Scalar&       plus_o)
{
    MatrixSizeCheck<2, 1>::check(p1);
    MatrixSizeCheck<2, 1>::check(p2);
    MatrixSizeCheck<2, 1>::check(plus_p);
 
    plus_p = p1 + p2;
    plus_o = pi2pi(o1 + o2);
}
 
template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void plus(const MatrixBase<D1>& p1,
                 const MatrixBase<D2>& o1,
                 const MatrixBase<D4>& p2,
                 const MatrixBase<D5>& o2,
                 MatrixBase<D7>&       plus_p,
                 MatrixBase<D8>&       plus_o)
{
    MatrixSizeCheck<1, 1>::check(o1);
    MatrixSizeCheck<1, 1>::check(o2);
    MatrixSizeCheck<1, 1>::check(plus_o);
 
    plus(p1, o1(0), p2, o2(0), plus_p, plus_o(0));
}
 
template <typename D1, typename D2, typename D3>
inline void plus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBase<D3>& d_plus)
{
    MatrixSizeCheck<3, 1>::check(d1);
    MatrixSizeCheck<3, 1>::check(d2);
    MatrixSizeCheck<3, 1>::check(d_plus);
 
    const auto                             p1 = d1.template head<2>();
    const auto                             p2 = d2.template head<2>();
    Map<Matrix<typename D3::Scalar, 2, 1>> p_p(&d_plus(0));
 
    plus(p1, d1(2), p2, d2(2), p_p, d_plus(2));
}
 
template <typename D1, typename D2>
inline Matrix<typename D1::Scalar, 3, 1> plus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2)
{
    Matrix<typename D1::Scalar, 3, 1> d_plus;
    plus(d1, d2, d_plus);
    return d_plus;
}
 
inline void plus(const VectorComposite& x, const VectorComposite& tau, VectorComposite& res)
{
    res['P'].resize(2);
    res['O'].resize(1);
    plus(x.at('P'), x.at('O'), tau.at('P'), tau.at('O'), res['P'], res['O']);
}
 
inline VectorComposite plus(const VectorComposite& x, const VectorComposite& tau)
{
    VectorComposite res;
    plus(x, tau, res);
    return res;
}
 
// MINUS (block-wise)
 
template <typename D1, typename D2, typename D3>
inline void minus(const MatrixBase<D1>&      p1,
                  const typename D1::Scalar& o1,
                  const MatrixBase<D2>&      p2,
                  const typename D2::Scalar& o2,
                  MatrixBase<D3>&            pd,
                  typename D3::Scalar&       od)
{
    MatrixSizeCheck<2, 1>::check(p1);
    MatrixSizeCheck<2, 1>::check(p2);
    MatrixSizeCheck<2, 1>::check(pd);
 
    pd = p2 - p1;
    od = pi2pi(o2 - o1);
}
 
template <typename D1, typename D2, typename D3>
inline void minus(const MatrixBase<D1>&      p1,
                  const typename D1::Scalar& o1,
                  const MatrixBase<D2>&      p2,
                  const typename D2::Scalar& o2,
                  MatrixBase<D3>&            pd,
                  typename D3::Scalar&       od,
                  typename D3::Scalar&       J_do_o1,
                  typename D3::Scalar&       J_do_o2)
{
    minus(p1, o1, p2, o2, pd, od);
 
    J_do_o1 = typename D3::Scalar(-1);
    J_do_o2 = typename D3::Scalar(1);
}
 
template <typename D1, typename D2, typename D4, typename D5, typename D7, typename D8>
inline void minus(const MatrixBase<D1>& p1,
                  const MatrixBase<D2>& o1,
                  const MatrixBase<D4>& p2,
                  const MatrixBase<D5>& o2,
                  MatrixBase<D7>&       pd,
                  MatrixBase<D8>&       od)
{
    MatrixSizeCheck<1, 1>::check(o1);
    MatrixSizeCheck<1, 1>::check(o2);
    MatrixSizeCheck<1, 1>::check(od);
 
    minus(p1, o1(0), p2, o2(0), pd, od(0));
}
 
template <typename D1, typename D2, typename D3>
inline void minus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2, MatrixBase<D3>& err)
{
    MatrixSizeCheck<3, 1>::check(d1);
    MatrixSizeCheck<3, 1>::check(d2);
    MatrixSizeCheck<3, 1>::check(err);
 
    const auto                             p1 = d1.template head<2>();
    const auto                             p2 = d2.template head<2>();
    Map<Matrix<typename D3::Scalar, 2, 1>> pd(&err(0));
 
    minus(p1, d1(2), p2, d2(2), pd, err(2));
}
 
template <typename D1, typename D2, typename D3, typename D4, typename D5>
inline void minus(const MatrixBase<D1>& d1,
                  const MatrixBase<D2>& d2,
                  MatrixBase<D3>&       dif,
                  MatrixBase<D4>&       J_diff_d1,
                  MatrixBase<D5>&       J_diff_d2)
{
    MatrixSizeCheck<3, 3>::check(J_diff_d1);
    MatrixSizeCheck<3, 3>::check(J_diff_d2);
 
    minus(d1, d2, dif);
 
    // block-wise difference: trivial jacobians
    J_diff_d1 = -Matrix<typename D4::Scalar, 3, 3>::Identity();
    J_diff_d2 = Matrix<typename D5::Scalar, 3, 3>::Identity();
}
 
template <typename D1, typename D2>
inline Matrix<typename D1::Scalar, 3, 1> minus(const MatrixBase<D1>& d1, const MatrixBase<D2>& d2)
{
    Matrix<typename D1::Scalar, 3, 1> ret;
    minus(d1, d2, ret);
    return ret;
}
 
inline void minus(const VectorComposite& x1, const VectorComposite& x2, VectorComposite& res)
{
    res['P'].resize(2);
    res['O'].resize(1);
    minus(x1.at('P'), x1.at('O'), x2.at('P'), x2.at('O'), res['P'], res['O']);
}
 
inline VectorComposite minus(const VectorComposite& x1, const VectorComposite& x2)
{
    VectorComposite res;
    minus(x1, x2, res);
    return res;
}
 
// INTERPOLATE
 
template <typename D1, typename D2, typename D3>
inline void interpolate(const MatrixBase<D1>&      d1,
                        const MatrixBase<D2>&      d2,
                        const typename D1::Scalar& t,
                        MatrixBase<D3>&            ret)
{
    MatrixSizeCheck<3, 1>::check(d1);
    MatrixSizeCheck<3, 1>::check(d2);
    MatrixSizeCheck<3, 1>::check(ret);
 
    Matrix<typename D1::Scalar, 3, 1> dd = between(d1, d2);
 
    Matrix<typename D1::Scalar, 3, 1> tau = t * log(dd);
 
    ret = compose(d1, exp(tau));
}
 
template <typename D1, typename D2>
inline Matrix<typename D1::Scalar, 3, 1> interpolate(const MatrixBase<D1>&      d1,
                                                     const MatrixBase<D2>&      d2,
                                                     const typename D1::Scalar& t)
{
    Matrix<typename D1::Scalar, 3, 1> ret;
    interpolate(d1, d2, t, ret);
    return ret;
}
 
inline void interpolate(const VectorComposite& x1, const VectorComposite& x2, const double& t, VectorComposite& res)
{
    VectorComposite tau = log(between(x1, x2));
    tau['P'] *= t;
    tau['O'] *= t;
    compose(x1, exp(tau), res);
}
 
inline VectorComposite interpolate(const VectorComposite& x1, const VectorComposite& x2, const double& t)
{
    VectorComposite res;
    interpolate(x1, x2, t, res);
    return res;
}
 
// HOMOGENEOUS MATRIX
 
template <typename D1, typename D2>
inline void toSE2(const MatrixBase<D1>& pose, MatrixBase<D2>& SE2)
{
    MatrixSizeCheck<3, 1>::check(pose);
    MatrixSizeCheck<3, 3>::check(SE2);
 
    typedef typename D1::Scalar T;
 
    SE2.template block<2, 1>(0, 2) = pose.template head<2>();
    SE2.template block<2, 2>(0, 0) = SO2::exp(T(pose(2)));
    SE2.template block<1, 2>(2, 0).setZero();
    SE2(2, 2) = (T)1.0;
}
 
template <typename D>
inline Matrix<typename D::Scalar, 3, 3> toSE2(const MatrixBase<D>& pose)
{
    Matrix<typename D::Scalar, 3, 3> SE2;
    toSE2(pose, SE2);
    return SE2;
}
 
}  // namespace SE2
}  // namespace wolf